/*
*Copyright (c) 2025 Huawei Device Co., Ltd. All rights reserved.
*Licensed under the Apache License, Version 2.0 (the "License");
*you may not use this file except in compliance with the License.
*You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

*Unless required by applicable law or agreed to in writing, software
*distributed under the License is distributed on an "AS IS" BASIS,
*WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*See the License for the specific language governing permissions and
*limitations under the License.
*/

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <opencv2/opencv.hpp>
#include <filesystem>
#include <chrono>

using namespace std::chrono_literals;
namespace fs = std::filesystem;

namespace apple_detect_demo {
class AppleDetector : public rclcpp::Node {
    // 参数
    std::string result_dir_;
    int min_contour_size_ = 150;
    std::vector<int> hsv_lower_;
    std::vector<int> hsv_upper_;
    static constexpr int HSV_CHANNEL_COUNT = 3;
    static constexpr int COLOR_MAX = 255;
    static constexpr int LABEL_OFFSET_Y = 5;        // 用于文本偏移的常量
    static constexpr double LABEL_FONT_SCALE = 0.7; // 用于文本字体缩放的常量
    static constexpr size_t kQueueSize = 10;
    double save_interval_ = 1.0;
    rclcpp::Time last_save_time_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;

public:
    AppleDetector() : Node("apple_detector")
    {
        // 参数设置
        result_dir_ = "detection_results";
        hsv_lower_ = { 0, 90, 128 };
        hsv_upper_ = { 180, 255, 255 };
        // 创建结果目录
        if (!fs::exists(result_dir_)) {
            fs::create_directory(result_dir_);
            RCLCPP_INFO(this->get_logger(), "Created result directory: %s", result_dir_.c_str());
        }

        subscription_ = this->create_subscription<sensor_msgs::msg::Image>("raw_images", kQueueSize,
            std::bind(&AppleDetector::image_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "Apple detector ready (save-only mode)");
    }

private:
    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        try {
            // 输入是bgr8格式,转换为OpenCV格式
            cv::Mat image(msg->height, msg->width, CV_8UC3,
                reinterpret_cast<uint8_t *>(const_cast<void *>(static_cast<const void *>(msg->data.data()))),
                msg->step);
            // 苹果检测逻辑
            cv::Mat hsv, mask;
            cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);
            cv::inRange(hsv, cv::Scalar(hsv_lower_[0], hsv_lower_[1], hsv_lower_[HSV_CHANNEL_COUNT - 1]),
                cv::Scalar(hsv_upper_[0], hsv_upper_[1], hsv_upper_[HSV_CHANNEL_COUNT - 1]), mask);

            std::vector<std::vector<cv::Point>> contours;
            cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

            // 绘制检测结果
            cv::Mat result = image.clone();
            for (const auto &cnt : contours) {
                if (cnt.size() < min_contour_size_)
                    continue;
                cv::Rect rect = cv::boundingRect(cnt);
                cv::rectangle(result, rect, cv::Scalar(0, COLOR_MAX, 0), HSV_CHANNEL_COUNT - 1);
                cv::putText(result, "Apple", cv::Point(rect.x, rect.y - LABEL_OFFSET_Y), cv::FONT_HERSHEY_SIMPLEX,
                    LABEL_FONT_SCALE, cv::Scalar(0, COLOR_MAX, 0), HSV_CHANNEL_COUNT - 1);
            }

            // 生成带时间戳的文件名
            auto now = std::chrono::system_clock::now();
            auto now_c = std::chrono::system_clock::to_time_t(now);
            std::stringstream ss;
            ss << result_dir_ << "/image_" << std::put_time(std::localtime(&now_c), "%Y%m%d_%H%M%S") << ".jpg";

            // 保存图像
            if (cv::imwrite(ss.str(), result)) {
                RCLCPP_INFO(this->get_logger(), "图像已保存: %s", ss.str().c_str());
            } else {
                RCLCPP_ERROR(this->get_logger(), "无法保存图像: %s", ss.str().c_str());
            }
        } catch (const std::exception &e) {
            RCLCPP_ERROR(this->get_logger(), "Processing error: %s", e.what());
        }
    }
};
}
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<apple_detect_demo::AppleDetector>());
    rclcpp::shutdown();
    return 0;
}